#include <iostream>
#include <cmath>
#include <limits>
#include <thread>

#include "serialPort.h"
#include "global.h"

// PID
// 控制器有三个参数，分别是比例系数（P）、积分系数（I）和微分系数（D）。这些参数的设置范围没有固定的标准，会因具体的控制系统、被控对象的特性以及控制要求的不同而有所差异。以下是一般情况下的设置范围及相关说明：
// 比例系数（P）
// 范围：通常在 0.1 到 100 之间，但在一些特殊情况下可能会超出这个范围。
// 说明：比例系数决定了控制器对误差的响应速度和力度。P
// 值越大，系统的响应速度越快，但可能会导致系统超调量增大，甚至出现振荡不稳定的情况；P
// 值越小，系统的响应速度越慢，稳态误差可能会较大。 积分系数（I）
// 范围：一般在 0.01 到 10 之间。
// 说明：积分系数主要用于消除系统的稳态误差。I
// 值越大，积分作用越强，消除稳态误差的速度越快，但可能会引起系统的稳定性下降，甚至导致系统发散；I
// 值越小，积分作用越弱，消除稳态误差的时间越长。 微分系数（D） 范围：通常在
// 0 到 1 之间，有时也可能会稍大一些。
// 说明：微分系数用于预测误差的变化趋势，提前对系统进行修正，从而改善系统的动态性能。D
// 值越大，微分作用越强，能有效抑制系统的超调，但如果 D
// 值过大，可能会使系统对噪声过于敏感，导致系统不稳定；D
// 值越小，微分作用越弱，对系统超调的抑制作用就不明显。
// PID 控制器类，实现了经典的 PID 控制算法
class PIDController
{
   private:
    double kp;  // 比例系数，用于根据当前误差调整控制输出
    double ki;  // 积分系数，用于消除系统的稳态误差
    double kd;  // 微分系数，用于预测误差的变化趋势，改善系统的动态响应
    double prev_error;      // 上一次计算时的误差，用于计算误差的微分
    double integral;        // 误差的积分值，不断累积误差
    double integral_limit;  // 积分限幅，防止积分项累积过大导致积分饱和

   public:
    // 构造函数，用于初始化 PID 控制器的参数
    // p: 比例系数
    // i: 积分系数
    // d: 微分系数
    // limit: 积分限幅
    PIDController(double p, double i, double d, double limit)
        : kp(p), ki(i), kd(d), prev_error(0), integral(0), integral_limit(limit)
    {
    }

    // 计算控制输出的函数
    // setpoint: 设定的目标值
    // current_value: 当前系统的实际值
    double calculate(double setpoint, double current_value)
    {
        // 计算当前误差，即目标值与实际值的差值
        double error = setpoint - current_value;
        if (error > 180)
        {
            error -= 360;
        }

        if (error < -180)
        {
            error += 360;
        }

        // 对误差进行积分，不断累加误差值
        integral += error;

        // 积分限幅，防止积分项累积过大导致积分饱和
        if (integral > integral_limit)
        {
            integral = integral_limit;
        }
        else if (integral < -integral_limit)
        {
            integral = -integral_limit;
        }

        // 计算误差的微分，即当前误差与上一次误差的差值
        double derivative = error - prev_error;

        // 根据 PID 算法的公式计算控制输出
        // 控制输出 = 比例项 + 积分项 + 微分项
        double output = kp * error + ki * integral + kd * derivative;

        // 更新上一次的误差值，为下一次计算微分做准备
        prev_error = error;

        return output;
    }

    // 重置积分项的函数，可在需要时将积分项清零
    void resetIntegral() { integral = 0; }
};

// 模拟云台系统的类，用于模拟云台的方位角和俯仰角控制
class GimbalClient
{
   private:
    double current_pan;   // 云台当前的方位角
    double current_tilt;  // 云台当前的俯仰角
    double pan_limit;     // 方位角的最大限制值，防止方位角超出合理范围
    double tilt_limit;    // 俯仰角的最大限制值，防止俯仰角超出合理范围

    const char *ptserial_fd = "/dev/ttyAMA4";
    serialPort ptserial;

    uint16_t zoom_bytes[25] = {0x0000, 0x03D7, 0x0851, 0x0C28, 0x10A3, 0x147A, 0x18F5,
                               0x1CCC, 0x2036, 0x22C5, 0x25C2, 0x28BF, 0x2B17, 0x2C96,
                               0x2DDD, 0x2F5C, 0x30A3, 0x3222, 0x3369, 0x34E8, 0x3666,
                               0x3A74, 0x3C96, 0x3DF9, 0x3F5C};

   public:
    // 构造函数，用于初始化云台的初始角度和角度限制
    // initial_pan: 云台的初始方位角
    // initial_tilt: 云台的初始俯仰角
    // pan_lim: 方位角的最大限制值
    // tilt_lim: 俯仰角的最大限制值
    GimbalClient(double initial_pan, double initial_tilt, double pan_lim, double tilt_lim)
        : current_pan(initial_pan),
          current_tilt(initial_tilt),
          pan_limit(pan_lim),
          tilt_limit(tilt_lim)
    {
        if (!ptserial.OpenPort("/dev/ttyAMA4", B19200))
        {
            custom_printf("Open serial deivce //dev//ttyAMA4\n");
        }

        if (system("bspmm 0x102600e0 0x1201 > /dev/null 2>&1") != 0)
        {
            custom_printf("System command failed\n");
        }
    }

    // 获取当前方位角的函数
    double getPan() const { return current_pan; }

    // 获取当前俯仰角的函数
    double getTilt() const { return current_tilt; }

    void start_thread()
    {
        custom_printf("Start gimbal thread\n");
        std::thread pt_thread(&GimbalClient::update_pt_absolute, this);
        pt_thread.detach();
    }

    bool update_pt_absolute()
    {
        while (true)
        {
            uint8_t buf[14] = {0};
            int nrecv = ptserial.readBuffer(buf, sizeof(buf));
            if (nrecv == 14)
            {
                uint8_t sum1 = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
                uint8_t sum2 = buf[8] + buf[9] + buf[10] + buf[11] + buf[12];

                if (buf[6] != sum1 || buf[13] != sum2)
                {
                    custom_printf("Sum error %d %d %d %d\n", buf[6], sum1, buf[13], sum2);
                    continue;
                }
                int pan_data = buf[4] * 256 + buf[5];
                current_pan = pan_data / 100.0;
                int tilt_data = buf[11] * 256 + buf[12];
                if (tilt_data >= 18000)
                {
                    tilt_data = 36000 - tilt_data;
                }
                else
                {
                    tilt_data = -tilt_data;
                }
                current_tilt = tilt_data / 100.0;
                // custom_printf("Update Pan: %.2f, Tilt: %.2f", current_pan,
                // current_tilt);
            }
            else
            {
                custom_printf("Read buffer error %d\n", nrecv);
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
        }
        return true;
    }

    // 根据控制输出更新云台角度的函数
    // pan_control: 用于控制方位角的控制输出
    // tilt_control: 用于控制俯仰角的控制输出
    void update_sim(double pan_control, double tilt_control)
    {
        // 这里简单模拟云台角度的更新，实际应用中需要根据云台特性调整
        // 乘以 0.1 是为了模拟云台的响应特性，避免角度变化过快
        current_pan += pan_control * 0.1;
        current_tilt += tilt_control * 0.1;

        // 方位角限幅，防止方位角超出设定的最大范围
        if (current_pan > pan_limit)
        {
            current_pan = pan_limit;
        }
        else if (current_pan < -pan_limit)
        {
            current_pan = -pan_limit;
        }

        // 俯仰角限幅，防止俯仰角超出设定的最大范围
        if (current_tilt > tilt_limit)
        {
            current_tilt = tilt_limit;
        }
        else if (current_tilt < -tilt_limit)
        {
            current_tilt = -tilt_limit;
        }
    }

    void control(double pan_control, double tilt_control)
    {
        constexpr int BUF_SIZE = 10;
        uint8_t buf[BUF_SIZE] = {0};

        // 包头和地址
        buf[0] = 0xa5;
        buf[1] = 0x01;

        // 方向控制
        if (std::abs(pan_control) > std::numeric_limits<double>::epsilon())
        {
            buf[2] |= (pan_control > 0) ? 0x40 : 0x80;  // 0x40: 右转，0x80: 左转
        }
        if (std::abs(tilt_control) > std::numeric_limits<double>::epsilon())
        {
            buf[2] |= (tilt_control > 0) ? 0x20 : 0x10;  // 0x20: 上升，0x10: 下降
        }

        // 时间参数
        buf[5] = 0x14;  // 200ms
        buf[8] = 0x14;  // 200ms

        // 速度计算和限制
        uint16_t pan_speed =
            std::min(static_cast<uint16_t>(fabs(pan_control) * 100), static_cast<uint16_t>(0xFFFF));
        uint16_t tilt_speed = std::min(static_cast<uint16_t>(fabs(tilt_control) * 100),
                                       static_cast<uint16_t>(0xFFFF));

        // 速度设置
        buf[3] = (pan_speed >> 8) & 0xFF;
        buf[4] = pan_speed & 0xFF;
        buf[6] = (tilt_speed >> 8) & 0xFF;
        buf[7] = tilt_speed & 0xFF;

        custom_printf("Control: pan=%.2f(0x%04X) tilt=%.2f(0x%04X)", pan_control, pan_speed,
                      tilt_control, tilt_speed);
        // custom_printf("Packet: %s", bytesToHexString(buf, BUF_SIZE).c_str());
        // custom_printf("0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x
        // 0x%02x 0x%02x", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6],
        // buf[7], buf[8], buf[9]);

        ptserial.writeBuffer(buf, sizeof(buf));
        // std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    bool set_zoom_ratio(uint8_t ratio)
    {
        if (ratio < 1 || ratio > 25)
        {
            return false;
        }
        printf("Set zoom ratio to %d\n", ratio);
        uint8_t buf[7] = {0};
        buf[0] = 0xFF;
        buf[1] = 0x01;
        buf[2] = 0x00;
        buf[3] = 0x4f;
        buf[4] = (zoom_bytes[ratio - 1] >> 8) & 0xFF;
        buf[5] = zoom_bytes[ratio - 1] & 0xFF;
        buf[6] = buf[1] + buf[2] + buf[3] + buf[4] + buf[5];
        ptserial.writeBuffer(buf, sizeof(buf));
        return true;
    }
};

void logStep(int step, double current_pan, double current_tilt, double target_pan,
             double target_tilt, double pan_control, double tilt_control)
{
    custom_printf(
        "Loop summary at Step %d: Current Pan = %.2f, Current Tilt = "
        "%.2f, Target Pan = %.2f, Target Tilt = %.2f, Pan Control = "
        "%.2f, Tilt Control = %.2f",
        step, current_pan, current_tilt, target_pan, target_tilt, pan_control, tilt_control);
}

int main(int argc, char **argv)
{
    const int loop_wait_time = 100;  // 控制循环的等待时间，单位为毫秒
    const int loop_count = 1000;     // 控制循环的次数，用于测试控制效果
    const int error_prescision = 5;  // 控制误差精度，单位为度

    // 解析target_pan和target_tilt参数，如果没有指定，则使用默认值
    double target_pan = 0.0;
    double target_tilt = 0.0;
    double target_zoom = 5.0;
    if (argc >= 4)
    {
        target_pan = std::stod(argv[1]);
        target_tilt = std::stod(argv[2]);
        target_zoom = std::stod(argv[3]);
    }

    // 初始化方位角 PID 控制器，设置比例系数为 1.0，积分系数为 0.1，微分系数为
    // 0.01，积分限幅为 100.0
    PIDController pan_pid(2.0, 0.1, 0.01, 100.0);
    // 初始化俯仰角 PID 控制器，设置比例系数为 1.0，积分系数为 0.1，微分系数为
    // 0.01，积分限幅为 100.0
    PIDController tilt_pid(2.0, 0.1, 0.01, 100.0);

    // 初始化云台，设置初始方位角和俯仰角为 0.0，方位角最大限制为 360.0
    // 度，俯仰角最大限制为 90.0 度
    GimbalClient gimbal_client(0.0, 0.0, 360.0, 90.0);
    gimbal_client.start_thread();

    std::this_thread::sleep_for(std::chrono::seconds(1));  // 等待云台初始化

    gimbal_client.set_zoom_ratio((uint8_t)target_zoom);

    // 控制循环，用于不断调整云台的方位角和俯仰角，使其趋近目标角度
    time_t start_time = time(nullptr);
    for (int i = 0; i < 1000; ++i)
    {
        // 步骤一，查询。获取云台当前的方位角和俯仰角
        double current_pan = gimbal_client.getPan();
        double current_tilt = gimbal_client.getTilt();
        custom_printf("Query: Current Pan = %.2f, Current Tilt = %.2f", current_pan, current_tilt);
        // 步骤二，如果误差都小于 2 度，认为已经达到目标角度，输出提示信息并退出循环
        int pan_error = std::abs(current_pan - target_pan);
        if (pan_error > 180)
        {
            pan_error = 360 - pan_error;
        }

        int tilt_error = std::abs(current_tilt - target_tilt);
        if (tilt_error > 90)
        {
            tilt_error = 180 - tilt_error;
        }

        if (pan_error < error_prescision && tilt_error < error_prescision)
        {
            gimbal_client.control(0.0, 0.0);
            std::cout << "Reached target angles! use step: " << i << " cost "
                      << (time(nullptr) - start_time) << std::endl;
            break;
        }

        // 步骤三计算pid输出。使用方位角 PID
        // 控制器计算控制输出，根据目标方位角和当前方位角进行计算
        double pan_control = pan_pid.calculate(target_pan, current_pan);
        double tilt_control = tilt_pid.calculate(target_tilt, current_tilt);

        // 步骤四， pid控制云台运动
        gimbal_client.control(pan_control, tilt_control);

        // 记录当前控制步骤的信息，包括步骤编号、当前方位角、当前俯仰角、方位角控制输出和俯仰角控制输出
        logStep(i, current_pan, current_tilt, target_pan, target_tilt, pan_control, tilt_control);
        std::this_thread::sleep_for(std::chrono::milliseconds(loop_wait_time));
    }

    return 0;
}